#include "fixedParticle.h"

const double EPSILON_TIME = 0.0000001;
const double EPSILON_DISTANCE = 0.01;

FixedParticle::FixedParticle(Vec3d initialPosition, Vec3d initialVelocity, double mass) : Particle(initialPosition, initialVelocity, mass) { 
	prevPosition = initialPosition;
	prevPositionTime = -100.0;
}

// Set a new position and compute our velocity from there.
void FixedParticle::setAndCompute(Vec3d newPosition, double time) {
	setPosition(newPosition, time);
	updateVelocity();
}

void FixedParticle::setPosition(Vec3d newPosition, double time) {

	if (time - prevPositionTime < EPSILON_TIME)  return;

	// Store old position
	prevPosition = position; 
	prevPositionTime = curPositionTime;
	
	// Set new position
	Particle::setPosition(newPosition);
	curPositionTime = time;
}

void FixedParticle::updateVelocity() {
	if ((position - prevPosition).length() < EPSILON_DISTANCE) return;
	Particle::setVelocity((position - prevPosition)/(curPositionTime - prevPositionTime));
}